
// 读取adc的数据  使用udock读取json数据

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "basic_sprayer_interfaces/msg/adc.hpp" //adc的Message
#include "basic_sprayer_interfaces/msg/human_cmd.hpp" // 控制信号

#include <QCoreApplication>
#include <QTimer>
#include <QSerialPort>
#include <QJsonDocument>
#include <QJsonParseError>
#include <QJsonObject>
#include <QDebug>

#include "crsf_protocol.h"

using namespace std::chrono_literals;

class Crc8
{
public:
    Crc8(uint8_t poly);
    uint8_t calc(uint8_t *data, uint8_t len);

protected:
    uint8_t _lut[256];
    void init(uint8_t poly);
};

Crc8::Crc8(uint8_t poly)
{
    init(poly);
}

void Crc8::init(uint8_t poly)
{
    for (int idx = 0; idx < 256; ++idx)
    {
        uint8_t crc = idx;
        for (int shift = 0; shift < 8; ++shift)
        {
            crc = (crc << 1) ^ ((crc & 0x80) ? poly : 0);
        }
        _lut[idx] = crc & 0xff;
    }
}

uint8_t Crc8::calc(uint8_t *data, uint8_t len)
{
    uint8_t crc = 0;
    while (len--)
    {
        crc = _lut[crc ^ *data++];
    }
    return crc;
}

/* 接收来自某个串口的数据 通过参数  */
class AdcJson : public QObject
{
public:
    rclcpp::Node::SharedPtr nh_;
    rclcpp::Publisher<basic_sprayer_interfaces::msg::HumanCmd>::SharedPtr publisher_;

    // rclcpp::PublisherOptions publisher_options_;
    explicit AdcJson(rclcpp::Node::SharedPtr &node, QObject *parent = nullptr) : QObject(parent)
    {
        nh_ = node;
        /* 指明参数 */
        rcl_interfaces::msg::ParameterDescriptor serial_descriptor;
        serial_descriptor.description = "which tty device to connect for adc";
        rclcpp::ParameterValue p_serial = nh_->declare_parameter("serial", rclcpp::ParameterValue("/dev/crsf_serial"), serial_descriptor);

        crc = new Crc8(0xd5);
        // publisher_ = nh_->create_publisher<basic_sprayer_interfaces::msg::Adc>("/hesiwei/adc", 10);
        publisher_ = nh_->create_publisher<basic_sprayer_interfaces::msg::HumanCmd>("/hesiwei/humancmd", 10);

        serial_port = new QSerialPort();
        std::string str;
        nh_->get_parameter("serial", str);
        RCLCPP_INFO(nh_->get_logger(), "connect tty:" + str);
        serial_port->setPortName(QString::fromStdString(str));
        serial_port->setBaudRate(420000, QSerialPort::AllDirections); // 设置波特率和读写方向
        serial_port->setDataBits(QSerialPort::Data8);                 // 数据位为8位
        serial_port->setFlowControl(QSerialPort::NoFlowControl);      // 无流控制
        serial_port->setParity(QSerialPort::NoParity);                // 无校验位
        serial_port->setStopBits(QSerialPort::OneStop);               // 一位停止位
        serial_port->close();

        //连接信号槽 当下位机发送数据QSerialPortInfo 会发送个 readyRead 信号,我们定义个槽void receiveInfo()解析数据
        connect(serial_port, &QSerialPort::readyRead, this, &AdcJson::readyRead);
        connect(serial_port, &QSerialPort::errorOccurred, this, &AdcJson::errorOccurred);

        /* 立即启动 */
        // timer = new QTimer(this);
        // connect(timer, &QTimer::timeout, this, &AdcJson::timeout);
        // timer->start(1000);
        timer_id_1s = startTimer(1000);
        timer_id_10ms = startTimer(10);
    }
    ~AdcJson()
    {
        serial_port->close();
    }

protected:
    void timerEvent(QTimerEvent *event)
    {
        if (timer_id_1s == event->timerId())
        {
            timeout();
        }
        else if (timer_id_10ms == event->timerId())
        {
            if (flag_rc)
            {
                flag_rc = false;
                flag_rc_cnt = 0;
            }
            else
            {
                flag_rc_cnt++;
                if (flag_rc_cnt > 10)
                {
                    rc_connect = false;
                }
            }
            auto message = basic_sprayer_interfaces::msg::HumanCmd();
            // message.data = "Hello, world! " + std::to_string(count_++);
            // RCLCPP_INFO(this->get_logger(), "%d %d %d %d", joy_status.connect_status, joy_status.start, joy_status.speed, joy_status.steer);
            if(joy_start == false){
                /*  */
                if(channels[0] < joy_mid_value){
                    joy_stop_cnt = 0;
                }else if((channels[0] > joy_mid_value) && (channels[2] < joy_low_value) && (joy_stop_cnt == 0) && (linkStatistics.uplink_Link_quality > 20)){
                    joy_start = true;
                    if( channels[1] < 500 ){
                        joy_dir = -1;
                    }else if(channels[1] > 1700){
                        joy_dir = 1;
                    }else{
                        joy_start = false;
                    }
                }
            }else{
                if(linkStatistics.uplink_Link_quality < 20){
                    joy_start = false;
                }
                if(channels[0] < joy_mid_value){
                    joy_start = false;
                }
                if(channels[2] < joy_low_value){
                    joy_stop_cnt ++;
                    if(joy_stop_cnt >= joy_stop_max){
                        joy_start = false;
                        joy_stop_cnt = joy_stop_max;
                    }
                }else{
                    joy_stop_cnt = 0;
                }
            }            
            message.header.stamp = nh_->now();
            if (rc_connect && usb_connect && joy_start)
            {
                message.cmd_switch = message.CMD_START;
                float speed = channels[2] - 300;
                
                message.velocity = speed > 0? joy_dir*speed * 2.f/1500 : 0;
                message.angle = - (channels[3] - 992) * (24.f / 818);
            }
            else
            {
                joy_start = false;
                message.cmd_switch = message.CMD_STOP;
                message.velocity = 0;
                message.angle = - (channels[3] - 992) * (24.f / 818);
            }
            publisher_->publish(message);
            /* 发布数据 */
        }
    }

private:
    void timeout()
    {
        if (serial_port->open(QIODevice::ReadWrite)) //用ReadWrite 的模式尝试打开串口
        {
            killTimer(timer_id_1s);
            timer_id_1s = -1;
            usb_connect = true;
            RCLCPP_INFO(nh_->get_logger(), "Connected");
        }
        else
        {
            RCLCPP_INFO(nh_->get_logger(), "trying");
        }
    }

    void errorOccurred(QSerialPort::SerialPortError error)
    {
        if (error == QSerialPort::ResourceError)
        {
            serial_port->close();
            usb_connect = false;
            if (timer_id_1s == -1)
            {
                timer_id_1s = startTimer(1000);
            }
            RCLCPP_INFO(nh_->get_logger(), "Disconnect");
        }
    }

    void readyRead()
    {
        if (serial_port->bytesAvailable())
        {
            QByteArray array = serial_port->readAll();
            // qDebug() << array;
            crsf_array.append(array);
            /* 验证协议 */
            /* 至少有4个 */
        again:
            if (crsf_array.size() > 4)
            {
                int length = (uint8_t)crsf_array[1]; //
                if (length > CRSF_MAX_PACKET_LEN + 4)
                {
                    /* 说明不对 */
                    crsf_array.remove(0, 1);
                    qDebug() << "11";
                    goto again;
                }
                if (crsf_array.size() < (length + 2))
                {
                    return;
                }
                /* 计算crc 结果 */
                if (crc->calc((uint8_t *)(crsf_array.data() + 2), length - 1) != (uint8_t)crsf_array[length + 1])
                {
                    crsf_array.remove(0, 1);
                    qDebug() << "22";
                    goto again;
                }

                /* 协议正确 */
                switch ((uint8_t)crsf_array[0])
                {
                case CRSF_ADDRESS_FLIGHT_CONTROLLER:
                {
                    if (address_flight_controller((uint8_t *)crsf_array.data()) == false)
                    {
                        qDebug() << "Do not process:" << (uint8_t) * (crsf_array.data() + 2);
                    }
                }
                break;

                default:
                    break;
                }

                crsf_array.remove(0, length + 2);
                goto again;
            }
        }
    }

    // bool
    int16_t channels[16] = {0};
    crsfLinkStatistics_t linkStatistics;

private:
    QSerialPort *serial_port = nullptr;
    QByteArray crsf_array; //读取全部数据
    Crc8 *crc;
    /* 时间  1s   10ms */
    int timer_id_1s = -1;
    int timer_id_10ms = -1;
    /* 连接状态 */
    bool flag_rc = false;
    int flag_rc_cnt = 0;
    bool rc_connect = false;
    bool usb_connect = false;
    const int joy_mid_value = 1024;
    const int joy_low_value = 300;

    bool joy_start = false;

    int joy_dir = 1;
    int joy_stop_cnt = 0;
    int joy_stop_max = 30*100;   //30s
    /* safe lock */

    bool address_flight_controller(uint8_t *crsfData)
    {
        switch (crsfData[2])
        {
        case CRSF_FRAMETYPE_GPS:
        {
        }
        break;
        case CRSF_FRAMETYPE_BATTERY_SENSOR:
        {
        }
        break;
        case CRSF_FRAMETYPE_LINK_STATISTICS:
        {
            /* 连接状态 */
            crsfLinkStatistics_t *sta = (crsfLinkStatistics_t *)&crsfData[3];
            memcpy(&linkStatistics, sta, sizeof(crsfLinkStatistics_t));
            // qDebug() << "downlink_Link_quality" << linkStatistics.downlink_Link_quality << linkStatistics.downlink_RSSI << linkStatistics.downlink_SNR << linkStatistics.uplink_Link_quality;
            return true;
        }
        break;
        case CRSF_FRAMETYPE_OPENTX_SYNC:
        {
        }
        break;
        case CRSF_FRAMETYPE_RADIO_ID:
        {
        }
        break;
        case CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
        {
            int16_t channels[16];
            channels[0] = ((crsfData[3] | crsfData[4] << 8) & 0x07FF);
            channels[1] = ((crsfData[4] >> 3 | crsfData[5] << 5) & 0x07FF);
            channels[2] = ((crsfData[5] >> 6 | crsfData[6] << 2 | crsfData[7] << 10) & 0x07FF);
            channels[3] = ((crsfData[7] >> 1 | crsfData[8] << 7) & 0x07FF);
            channels[4] = ((crsfData[8] >> 4 | crsfData[9] << 4) & 0x07FF);
            channels[5] = ((crsfData[9] >> 7 | crsfData[10] << 1 | crsfData[11] << 9) & 0x07FF);
            channels[6] = ((crsfData[11] >> 2 | crsfData[12] << 6) & 0x07FF);
            channels[7] = ((crsfData[12] >> 5 | crsfData[13] << 3) & 0x07FF); // & the other 8 + 2 channels if you need them
            channels[8] = ((crsfData[14] | crsfData[15] << 8) & 0x07FF);
            channels[9] = ((crsfData[15] >> 3 | crsfData[16] << 5) & 0x07FF);
            channels[10] = ((crsfData[16] >> 6 | crsfData[17] << 2 | crsfData[18] << 10) & 0x07FF);
            channels[11] = ((crsfData[18] >> 1 | crsfData[19] << 7) & 0x07FF);
            channels[12] = ((crsfData[19] >> 4 | crsfData[20] << 4) & 0x07FF);
            channels[13] = ((crsfData[20] >> 7 | crsfData[21] << 1 | crsfData[22] << 9) & 0x07FF);
            channels[14] = ((crsfData[22] >> 2 | crsfData[23] << 6) & 0x07FF);
            channels[15] = ((crsfData[23] >> 5 | crsfData[24] << 3) & 0x07FF);
            memcpy(this->channels, channels, sizeof(this->channels));
            /* 接收到rc的数据 */
            flag_rc = true;
            rc_connect = true;
            // for (int i = 0; i < 26; i++)
            // {
            //     printf("%d ", this->channels[i]);
            // }
            // std::cout << std::endl;

            return true;
        }
        break;
        case CRSF_FRAMETYPE_ATTITUDE:
        {
        }
        break;
        case CRSF_FRAMETYPE_FLIGHT_MODE:
        {
        }
        break;
        case CRSF_FRAMETYPE_DEVICE_PING:
        {
        }
        break;
        case CRSF_FRAMETYPE_DEVICE_INFO:
        {
        }
        break;
        case CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY:
        {
        }
        break;
        case CRSF_FRAMETYPE_PARAMETER_READ:
        {
        }
        break;
        case CRSF_FRAMETYPE_PARAMETER_WRITE:
        {
        }
        break;
        case CRSF_FRAMETYPE_COMMAND:
        {
        }
        break;

        case CRSF_FRAMETYPE_MSP_REQ:
        {
        }
        break;

        case CRSF_FRAMETYPE_MSP_RESP:
        {
        }
        break;
        case CRSF_FRAMETYPE_MSP_WRITE:
        {
        }
        break;

        default:
        {
        }
        break;
        }
        return false;
    }
};

void gquit(int sig)
{
    (void)sig;
    rclcpp::shutdown();
    QCoreApplication::exit(0);
}

class ADCApp : public QCoreApplication
{
public:
    rclcpp::Node::SharedPtr nh_;
    explicit ADCApp(int &argc, char **argv)
        : QCoreApplication(argc, argv)
    {
        rclcpp::init(argc, argv);
        nh_ = rclcpp::Node::make_shared("crsf_joystick_publisher");
        signal(SIGINT, gquit);
    }

    ~ADCApp()
    {
        rclcpp::shutdown();
    }

    int exec()
    {
        AdcJson adc(nh_);
        return QCoreApplication::exec();
    }
};

int main(int argc, char *argv[])
{
    ADCApp app(argc, argv);
    return app.exec();
}
